#include "toroidal.h"
#include <math.h>
#include <stdio.h>

double field::interp(cylv vp) // need modification at boundary
{
    double r = vp.r;
    double z = vp.z;
    int rg = (int)((r - Rmin)/dr);
    int zg = (int)((z - Zmin)/dz);
    double ofr = (r - (double)rg * dr - Rmin)/dr;
    double ofz = (z - (double)zg * dz - Zmin)/dz;
    double ret = 0.0;
    double coefrs[4] = {ofr*(ofr-1.0)*(ofr-2.0)/-6.0, (ofr+1.0)*(ofr-1.0)*(ofr-2.0)/2.0,
    (ofr+1.0)*ofr*(ofr-2.0)/-2.0, (ofr+1.0)*ofr*(ofr-1.0)/6.0};
    double coefzs[4] = {ofz*(ofz-1.0)*(ofz-2.0)/-6.0, (ofz+1.0)*(ofz-1.0)*(ofz-2.0)/2.0,
    (ofz+1.0)*ofz*(ofz-2.0)/-2.0, (ofz+1.0)*ofz*(ofz-1.0)/6.0};
    for(int i = 0; i < 4; i++)
        for(int j = 0; j < 4; j++)
        {
            int point = 0;
            if(rg+i-1 < 0)
                point += Ny*(rg+i);
            else if(rg+i-1 > Nx)
                point += Ny*(rg+i-2);
            else
                point += Ny*(rg+i-1);
            if (zg+j-1 < 0)
                point += zg+j;
            else if (zg+j-1 > Nx)
                point += zg+j-2;
            else
                point += zg+j-1;
            ret += coefrs[i]*coefzs[j]*data[point];
        }
    return ret;
}

void ptcs::scatter(double* pdata) 
{
    for(int i = 0; i < Nx*Ny; i++)
        pdata[i] = 0.000;
    for(int pt = 0; pt < npt; pt++)
    {
        if(this->inside[pt] == false)
            continue; 
        cylv vp = ppts[pt].getcrd();
        int rg = (int)((vp.r - Rmin)/dr);
        int zg = (int)((vp.z - Zmin)/dz);
        double ofr = (vp.r - (double)rg * dr - Rmin)/dr;
        double ofz = (vp.z - (double)zg * dz - Zmin)/dz;
        double coefrs[2] = {ofr, 1.0-ofr};
        double coefzs[2] = {ofz, 1.0-ofz};
        double psi = 0.0;
    
        for(int i = 0; i < 2; i++)
        for(int j = 0; j < 2; j++) {
            int point = Ny*(rg+i)+ zg+j;
            double wg = wdelta[pt];
            pdata[point] += coefrs[i]*coefzs[j]*wg*sin(3*vp.p);
        }
    }

}

extern field Apsi;
bool checkboundary(cylv vp)
{
    double pr = vp.r;
    double pz = vp.z;
    if (pr > Rmax || pr < Rmin)
        return false;
    if (pz > Zmax || pz < Zmin)
        return false;
    double psi = Apsi.interp(vp);
    if (psi == 0.0)
        return false;
    return true;
}

bool checkboundary_core(cylv vp)
{
    double pr = vp.r;
    double pz = vp.z;
    if (pr > Rmax || pr < Rmin)
        return false;
    if (pz > Zmax || pz < Zmin)
        return false;
    double psi = Apsi.interp(vp);
    if (psi/psiaxis < 0.1)
        return false;
    return true;
}

vec operator+(vec a, vec b)
{
    vec c = {0.0,0.0,0.0};
    c.x = a.x + b.x;
    c.y = a.y + b.y;
    c.z = a.z + b.z;
    return c;
}

vec operator*(double a, vec x)
{
    vec y = {0.0, 0.0, 0.0};
    y.x = a*x.x;
    y.y = a*x.y;
    y.z = a*x.z;
    return y;
}
double dot(vec a, vec b)
{
    return a.x*b.x + a.y*b.y + a.z*b.z;
}

vec cross(vec a, vec b) //right hand coordinate system!!!
{
    return {a.y*b.z-a.z*b.y, a.z*b.x-a.x*b.z, a.x*b.y-a.y*b.x};
}

void print(vec a)
{
    printf("%lf,%lf,%lf\n",a.x,a.y,a.z);
}

vec cyl2xyz(cylv a)
{
    return {a.r*cos(a.p),a.r*sin(a.p),a.z};
}
cylv xyz2cyl(vec a)
{
    double pphi = atan(a.y/a.x) + pi*(a.x<0.0) + 2*pi*(a.y < 0.0 && a.x > 0.0);
    return {sqrt(pow(a.x,2)+pow(a.y,2)), pphi, a.z};
}

vec rotate(vec a, double phi)
{
    return {a.x*cos(phi)+a.y*sin(phi),a.y*cos(phi)-a.x*sin(phi),a.z};
}

